#pragma once
#include <chrono>
#include <rclcpp/rclcpp.hpp>
#include "fishbot_behavior_tree/async_behavior_base.hpp"

namespace fishbot
{
    namespace behavior_tree
    {
        template <typename ServiceT>
        class ClientInterfaceBase
        {
        public:
            virtual ~ClientInterfaceBase() = default;
            virtual void initialize(const std::string &service_name,
                                    std::chrono::duration<double> wait_for_timeout,
                                    std::chrono::duration<double> response_timeout) = 0;
            virtual bool waitForServiceServer() const = 0;
            virtual tl::expected<typename ServiceT::Response, std::string> syncSendRequest(const typename ServiceT::Request &request) = 0;
            virtual void cancelRequest() = 0;
        };

        template <typename ServiceT>
        class RclcppClientInterface : public ClientInterfaceBase<ServiceT>
        {
        public:
            RclcppClientInterface(const std::shared_ptr<rclcpp::Node> node) : node_(node), client_(nullptr)
            {
            }
            void initialize(const std::string &service_name,
                            std::chrono::duration<double> wait_for_timeout,
                            std::chrono::duration<double> response_timeout) override
            {
                client_ = node_->create_client<ServiceT>(service_name);
                wait_for_timeout_ = wait_for_timeout;
                response_timeout_ = response_timeout;
            }

            bool waitForServiceServer() const override
            {
                return client_->wait_for_service(wait_for_timeout_);
            }

            tl::expected<typename ServiceT::Response, std::string> syncSendRequest(const typename ServiceT::Request &request) override
            {
                if (!client_->service_is_ready())
                {
                    return tl::make_unexpected("Service server not available");
                }
                auto shared_request = std::make_shared<typename ServiceT::Request>(request);
                auto result_future = client_->async_send_request(shared_request);
                if (rclcpp::spin_until_future_complete(node_, result_future, response_timeout_) ==
                    rclcpp::FutureReturnCode::SUCCESS)
                {
                    return *result_future.get();
                }
                else
                {
                    return tl::make_unexpected("Service request timed out.");
                }
            }

            void cancelRequest() override {}

        private:
            std::shared_ptr<rclcpp::Node> node_;
            typename rclcpp::Client<ServiceT>::SharedPtr client_;
            std::chrono::duration<double> wait_for_timeout_;
            std::chrono::duration<double> response_timeout_;
        };

        template <typename ServiceT>
        class ServiceClientBehaviorBase : public AsyncBehaviorBase
        {
        public:
            ServiceClientBehaviorBase(const std::string &name, const BT::NodeConfiguration &config,
                                      const std::shared_ptr<BehaviorContext> &shared_resources)
                : AsyncBehaviorBase(name, config, shared_resources)
            {
                client_interface_ = std::make_unique<RclcppClientInterface<ServiceT>>(shared_resources->node_);
            }

            virtual ~ServiceClientBehaviorBase() = default;

        protected:
            virtual tl::expected<std::chrono::duration<double>, std::string> getServerTimeout()
            {
                auto timeout = getInput<double>("wait_for_server_timeout");
                if (!timeout)
                {
                    RCLCPP_WARN(rclcpp::get_logger("ServiceClientBehaviorBase"), "Input wait_for_server_timeout is missing, set to default 5s");
                    return std::chrono::duration<double>(5.0);
                }
                return std::chrono::duration<double>(timeout.value());
            }

            virtual tl::expected<std::chrono::duration<double>, std::string> getResponseTimeout()
            {
                auto timeout = getInput<double>("result_timeout");
                if (!timeout)
                {
                    RCLCPP_WARN(rclcpp::get_logger("ServiceClientBehaviorBase"), "Input result_timeout is missing, set to default 10s");
                    return std::chrono::duration<double>(10.0);
                }
                return std::chrono::duration<double>(timeout.value());
            }

            virtual tl::expected<std::string, std::string> getServiceName() = 0;
            virtual tl::expected<typename ServiceT::Request, std::string> createRequest() = 0;
            virtual tl::expected<bool, std::string> processResponse(const typename ServiceT::Response &)
            {
                return true;
            }

            tl::expected<bool, std::string> doWork() override
            {
                // 获取service name, response timeout, 初始化client interface
                auto service_name = getServiceName();
                if (!service_name)
                {
                    return tl::make_unexpected(service_name.error());
                }

                auto response_timeout = getResponseTimeout();
                if (!response_timeout)
                {
                    return tl::make_unexpected(response_timeout.error());
                }

                auto server_timeout = getServerTimeout();
                if (!server_timeout)
                {
                    return tl::make_unexpected(server_timeout.error());
                }

                client_interface_->initialize(service_name.value(), server_timeout.value(), response_timeout.value());

                // 创建请求内容
                auto request = createRequest();
                if (!request)
                {
                    return tl::make_unexpected("Create request failed: " + request.error());
                }

                // 等待service
                if (!client_interface_->waitForServiceServer())
                {
                    return tl::make_unexpected("Service server is not ready: " + service_name.value());
                }
                
                // 发送请求，等待返回结果
                auto response = client_interface_->syncSendRequest(request.value());
                if (!response)
                {
                    return tl::make_unexpected("Failed to send service request");
                }

                // 处理结果
                if (!processResponse(response.value()))
                {
                    return tl::make_unexpected("Failed to process service response");
                }
                else
                {
                    return true;
                }
            }

            std::shared_future<tl::expected<bool, std::string>> &getFuture() override
            {
                return future_;
            }
            std::shared_future<tl::expected<bool, std::string>> future_;

        private:
            std::unique_ptr<ClientInterfaceBase<ServiceT>> client_interface_;
        };
    }
}